#include "testApp.h"
#include "stdio.h"

testApp::testApp(ofxArgs* args){
	this->args = args;
}

//--------------------------------------------------------------
void testApp::setup(){
	version = "0.9.6";
	cout<<"BLIND v. "+version<<endl<<"-------"<<endl<<endl;

	slog("Begin setup");
	//this->args->printOpts();
	for(int i=0;i<40;i++){
		curCue[i] = -1;
	}

    computerNumber = this->args->getInt(1);
    cout<<"Computer number: "<<computerNumber<<endl;
	//Setup sound
#if defined( TARGET_OSX )
	soundFilePath = "/WorldrippleSound/";
	oscControlSender.setup("halfdanj.local", 14000+computerNumber-1);

#endif
#if  defined( TARGET_LINUX )
	soundFilePath = "/home/worldripple/narve.wav";
#endif
#if  defined( TARGET_WIN32 )
	soundFilePath = "/lyd/";
	oscControlSender.setup("halfdanj", 14000+computerNumber-1);

#endif
	soundFiles[0] = "Vero.wav";
	soundFiles[1] = "Narve.wav";
	soundFiles[2] = "Edy.wav";
	soundFiles[3] = "Kate.wav";
	soundFiles[4] = "Shiva.wav";

	string xmlFile = "timeline.xml";


	soundFile = soundFiles[computerNumber-1];

	if(computerNumber == 1){
//		soundFile = soundFiles[1];
		folder = "Narve/";
		xmlFile = "narvetimeline.xml";
	}
	loadSound(soundFilePath+soundFile);
	cout<<"Connecting to OSC..."<<endl;
	//Osc
	oscRecv.setup(12001);
	oscSender.setup("localhost", 12000);
	oscRecvTimeline.setup(12002);
	oscSenderTimeline.setup("localhost", 12003);

	oscControlRecv.setup(13000+computerNumber-1);

    cout<<"Connected. Loading XML timeline"<<endl;

	//XML
	XML.loadFile(soundFilePath+folder+xmlFile);
	XML.pushTag("CUES");
	numCues = XML.getNumTags("CUE");
	cout<<"Loaded timeline "<<soundFilePath+xmlFile<<" with "<<numCues<<" cues"<<endl;

	//Cues
	/*	for(int i=0;i<20;i++){
	 cues.push_back(noneCue(0,i));
	 }*/
	for(int i=0;i<numCues;i++){
		Cue * newCue;
		int time = XML.getAttribute("CUE","minutes",0.0,i) * 60.0 + XML.getAttribute("CUE","seconds",0.0,i);
		if (strcmp(XML.getAttribute("CUE","type","none",i).c_str(), "random") == 0) {
			newCue = new randomCue(time, XML.getAttribute("CUE","layer",0,i));
			((randomCue*)newCue)->soft = XML.getAttribute("CUE","soft",0,i);
		} else if (strcmp(XML.getAttribute("CUE","type","none",i).c_str(), "duskfile") == 0) {
			newCue = new duskFileCue(time, XML.getAttribute("CUE","layer",0,i));
			((duskFileCue*)newCue)->loop = XML.getAttribute("CUE","loop",0,i);
			((duskFileCue*)newCue)->file = new ofxXmlSettings();
			((duskFileCue*)newCue)->file->loadFile(soundFilePath+folder+XML.getAttribute("CUE","file","",i));
		} else if (strcmp(XML.getAttribute("CUE","type","none",i).c_str(), "acc") == 0) {
			newCue = new accControlCue(time, XML.getAttribute("CUE","layer",0,i));
			((accControlCue*)newCue)->level = &accVolume;
			((accControlCue*)newCue)->dusk = XML.getAttribute("CUE","dusk",0,i);
			((accControlCue*)newCue)->audio = XML.getAttribute("CUE","audio",0,i);
		} else {
			newCue = new noneCue(time, XML.getAttribute("CUE","layer",0,i));
		}

		XML.pushTag("CUE",i);
		int numGroups = 0;
		if(XML.getNumTags("GROUPS") > 0){
			XML.pushTag("GROUPS");

			//Groups
			numGroups =  XML.getNumTags("GROUP");
			for(int u=0;u<numGroups;u++){
				for(int g=XML.getAttribute("GROUP","begin", 0,u);g<=XML.getAttribute("GROUP","end", 0,u);g++){
					newCue->affects.push_back(g);
				}
			}

			XML.popTag();
		}
		if(numGroups == 0){
			for(int g=0;g<64;g++){
				newCue->affects.push_back(g);
			}
		}

		//Amps
		int numAmps = 0;
		if(XML.getNumTags("AMPS") > 0){
			XML.pushTag("AMPS");
			numAmps =  XML.getNumTags("AMP");
			for(int u=0;u<numAmps;u++){
				newCue->amps.push_back(Amp(XML.getAttribute("AMP","value", 100.0,u),XML.getAttribute("AMP","time", 0.0 ,u)));
			}

			XML.popTag();
		}
		XML.popTag();
		if(numAmps == 0){
			newCue->amps.push_back(Amp(100));
		}



		cues.push_back(newCue);


	}
	XML.popTag();





    cout<<"Connecting to arduino"<<endl;

	//Serial
	//serial.enumerateDevices();
#if  defined( TARGET_LINUX )
	serialConnected = serial.setup(0, 115200);
#endif
#if  defined( TARGET_OSX )
	serialConnected=serial.setup(6, 115200);
#endif
#if  defined( TARGET_WIN32 )
	serialConnected=serial.setup(0,115200);
#endif
	if(!serialConnected){
		slog("No arduino found!");
		sound.play();
    } else {
		slog("Connected to arduino");
    }

	arduinoBufferSize = 0;
	shortSum = -1;
	msgLock = false;
	lstMsg = 0; //last message sent
	lstMsgType = 0; //if last message wasn't a type identifier, type is specified here
	msgTimer = 0; //time since last message was sent
	connectedToArduino = false;
	inputTimeline = false;
	intputAccVolume = false;
	intputRandomNoise = false;
	intputSmoothRandomNoise = false;
	intputNone = true;
	lowPass = 0;
	accVolume = 0;
	receivingAcc = false;
	numBytes = 0;
	masterVolume =  1.0;
	masterVibration = 1.0;
	ac1 = 0;
	panOnAcc = false;

	//Sound
	//	loadSound(soundFilePath+soundFiles[0]);
	//sound.play();

	for(int i=0;i<64;i++){
		duskValues[i] = 0;
		duskSendValues[i] = 0;
		duskOutputs[i] = 0;
	}

	memset(bytesReturned, 0, 4);
	memset(bytesReturned2, 0, 5);


	ofSetFrameRate(30);

	slog("End setup");
	slog("");
	button = false;
	curTime = 0;

	for(int i=0;i<3;i++){
		curAcc0[i] = 0;
	}

}

//--------------------------------------------------------------
void testApp::update(){
	ofxOscMessage m;
	m.setAddress( "/status" );
	m.addIntArg( serialConnected );
	m.addIntArg(sound.getIsPlaying());
	m.addFloatArg( (float)sound.getPosition());
	m.addFloatArg( (float)sound.length/sound.internalFreq);
	m.addStringArg(soundFile);


	int highestCue = -1;
	for(int i=0;i<20;i++){
		if(curCue[i] > highestCue)
			highestCue = curCue[i];
	}
	m.addIntArg(highestCue);
	m.addIntArg(numCues);

	m.addFloatArg( curAcc0[0]);
	m.addFloatArg( curAcc0[1]);
	m.addFloatArg( curAcc0[2]);
	m.addFloatArg( accVolume);
	m.addStringArg(version);

	oscControlSender.sendMessage(m);

	if(ofGetFrameNum() % 100 == 0){

		ofxOscMessage m;
		m.setAddress( "/player/acc" );
		m.addIntArg( curAcc0[0]);

		m.addIntArg( curAcc0[1]);
		m.addIntArg( curAcc0[2]);

		oscSender.sendMessage( m );
		receivingAcc = false;

	}

	while( oscRecv.hasWaitingMessages() )
	{
		// get the next message
		ofxOscMessage m;
		oscRecv.getNextMessage( &m );
		//		cout<< m.getAddress().c_str()<<endl;
		/*if( strcmp( m.getAddress().c_str(), "/player/file" ) == 0){
		 if(m.getArgAsInt32( 0) == -1){
		 sound.unloadSound();
		 soundLoaded = false;
		 } else {
		 loadSound(soundFilePath+soundFiles[ m.getArgAsInt32( 0)]);
		 }

		 }
		 */
		if( strcmp( m.getAddress().c_str(), "/player/panacc" ) == 0){
			panOnAcc = m.getArgAsInt32( 0);
		}
		if( strcmp( m.getAddress().c_str(), "/player/lowpass" ) == 0){
			lowPass = m.getArgAsInt32( 0);
		}
		if( strcmp( m.getAddress().c_str(), "/player/masterVibration" ) == 0){
			masterVibration = m.getArgAsFloat( 0);
		}
		if( strcmp( m.getAddress().c_str(), "/player/masterVolume" ) == 0){
			masterVolume = m.getArgAsFloat( 0);
		}

		if( strcmp( m.getAddress().c_str(), "/editor/play" ) == 0){
			slog("Remote plays sound from "+ofToString(m.getArgAsFloat( 0), 2));
			sound.play();
			sound.setPosition(m.getArgAsFloat( 0));
		}
		if( strcmp( m.getAddress().c_str(), "/editor/pause" ) == 0){
			slog("Remote pauses sound");
			sound.stop();
		}
		if( strcmp( m.getAddress().c_str(), "/editor/position" ) == 0){
			sound.setPosition((float)(m.getArgAsInt32( 0)/1000.0)/(sound.length/sound.internalFreq));
		}
		if( strcmp( m.getAddress().c_str(), "/dusk/value" ) == 0){
			if(!inputTimeline)
				duskValues[m.getArgAsInt32( 0)] = m.getArgAsInt32( 1);
		}
		if( strcmp( m.getAddress().c_str(), "/dusk/values" ) == 0){
			if(!inputTimeline){
				for(int i=0;i<64;i++){
					duskValues[i] = m.getArgAsInt32( i);
				}
			}
		}
		if( strcmp( m.getAddress().c_str(), "/dusk/input" ) == 0){
			int n = m.getArgAsInt32(0);
			if(n == 0){
				intputNone = true;
				slog("Input: None");
			} else {
				intputNone = false;
			}
			if(n == 1){
				inputTimeline = true;
				slog("Input: Timeline");
			} else {
				inputTimeline = false;
			}
			if(n == 2){
				intputRandomNoise = true;
				slog("Input: Random noise");
			} else {
				intputRandomNoise = false;
			}
			if(n == 3){
				intputSmoothRandomNoise = true;
				slog("Input: Random smooth noise");
			} else {
				intputSmoothRandomNoise = false;
			}



			ofxOscMessage m;
			m.setAddress( "/timeline/active" );
			m.addIntArg(inputTimeline);
			oscSenderTimeline.sendMessage( m );
		}
		if( strcmp( m.getAddress().c_str(), "/dusk/inputAcc" ) == 0){
			bool n = m.getArgAsInt32(0);
			if(n == true){
				intputAccVolume = true;
				slog("Input: Acc Control");
			} else {
				intputAccVolume = false;
			}

		}

	}

	while( oscRecvTimeline.hasWaitingMessages() )
	{
		// get the next message
		ofxOscMessage m;
		oscRecvTimeline.getNextMessage( &m );
		//				cout<< m.getAddress().c_str()<<endl;
		if( strcmp( m.getAddress().c_str(), "/editor/play" ) == 0){
			//			slog("Editor plays sound");
			//			sound.play();
			//			sound.setPosition((float)(m.getArgAsInt32( 0)/1000.0)/(sound.length/sound.internalFreq));
		}
		if( strcmp( m.getAddress().c_str(), "/editor/pause" ) == 0){
			//			slog("Editor pauses sound");
			//			sound.stop();
		}
		if( strcmp( m.getAddress().c_str(), "/editor/position" ) == 0){
			//			sound.setPosition((float)(m.getArgAsInt32( 0)/1000.0)/(sound.length/sound.internalFreq));
		}
		if( strcmp( m.getAddress().c_str(), "/timeline/value" ) == 0){
			if(inputTimeline)
				duskValues[m.getArgAsInt32( 0)] = m.getArgAsInt32( 1)*volume;
		}
	}

	if(lastVolume != volume*masterVolume){
		sound.setVolume((float)volume*masterVolume);
		lastVolume = volume*masterVolume;
	}


	if(!inputTimeline){
		//Output the values to the gui
		ofxOscMessage m;
		m.setAddress( "/timeline/values" );
		for(int i=0;i<64;i++){
			m.addIntArg(duskValues[i]);
		}
		oscSenderTimeline.sendMessage( m );
	}
	if(intputNone){
		for(int i=0;i<64;i++){
			duskValues[i] = 0;
		}

	}
	intputRandomNoise = true;
	if(intputRandomNoise){
		for(int i=0;i<64;i++){
			duskValues[i] = (ofRandom(0,160)*volume);
		}
	}
	if(intputSmoothRandomNoise){
		for(int i=0;i<64;i++){
			//	duskValues[i] = (ofRandom(0,160)*volume);
			duskValues[i] = volume*((sin(ofGetElapsedTimeMillis()/1000.0+i*210)+1.0)/2.0)*120.0;

		}
	}

	if(acc0.size() > 6){
		int lastAcc[3];
		lastAcc[2] = acc0.at(acc0.size() -4);
		lastAcc[1] = acc0.at(acc0.size() -5);
		lastAcc[0] = acc0.at(acc0.size() -6);
	//	level *= 0.8;
	//	level += ((float)fabs(curAcc0[0] - lastAcc[0]) + fabs(curAcc0[1] - lastAcc[1]) + fabs(curAcc0[2] - lastAcc[2]))*0.2;
		level = ((float)fabs(curAcc0[0] - lastAcc[0]) + fabs(curAcc0[1] - lastAcc[1]) + fabs(curAcc0[2] - lastAcc[2]));
		accVolume += (((float)level/7.0-accVolume)-0.35*2.0)*0.015;
		if(accVolume > 1.0)
			accVolume = 1.0;
		if(accVolume < 0.0)
			accVolume = 0.0;
		ac1 += (curAcc0[1]/60.0-ac1)*0.02;
		if(panOnAcc)
			sound.setPan(-ac1);
		//	cout<<curAcc0[1]<<endl;
		ofxOscMessage m;
		m.setAddress( "/player/level" );
		m.addFloatArg(accVolume);
		oscSender.sendMessage( m );
	}

	if(intputAccVolume && acc0.size() > 6){
		if(accVolume == 0.0 && sound.getIsPlaying()){
			//		resumePoint = sound.getPosition();
			//		sound.stop();
		} else if(!sound.getIsPlaying() && accVolume > 0.0){
			//		sound.play();
			//		sound.setPosition(resumePoint);
		}
		volume = accVolume;
	}  else {
		volume = 1.0f;
	}

	if(sound.getIsPlaying()){
		ofxOscMessage m;
		m.setAddress( "/player/time" );
		//		m.addIntArg( (float)sound.getPosition()*(sound.length/sound.internalFreq)*1000.0 );
		m.addFloatArg( (float)sound.getPosition());
		oscSender.sendMessage( m );
	}

	for(int i=0;i<64;i++){
		if(duskValues[i]<lowPass){
			duskValues[i] = 0;
		}
	}


	for(int i=0;i<64;i++){
		duskValues[i] = 0;
	}

	//curTime = sound.getPosition();


	for(int i=0;i<20;i++){
		int bestCue = -1;
		for(int u=0;u<cues.size();u++){
			if(cues[u]->layer == i){
				if(cues[u]->startTime <= curTime){
					if(bestCue == -1){
						bestCue = u;
					} else if(cues[u]->startTime >= cues[bestCue]->startTime){
						bestCue = u;
					}
				}
			}
		}

		if(bestCue != -1){
			if(curCue[i] != bestCue){
				cout<<curTime<<": Cue "<<bestCue<<endl;
				curCue[i] = bestCue;
			}
			cues[bestCue]->doStuff(curTime, &sound, duskValues);
		}
	}



    if(sound.getIsPlaying()){
        curTime = sound.getPosition()*sound.length/(float)sound.internalFreq;
        //cout<<curTime<<endl;
    }
	if(serialConnected){
		bool gotMsg = false;
		//int nRead  = 0;  // a temp variable to keep count per read
		//cout<<serial.available()<<endl;
		unsigned char recv;
		while( serial.available()){
			gotMsg = true;
			numBytes ++;
			recv = serial.readByte();
			//			memcpy(bytesReturned2, bytesReturned, 4);

			bytesReturned2[numBytes-1] = recv;
			//	cout<<"-"<<bytesReturned2[0]<<"    "<<numBytes-1<<endl;
			//	cout<<numBytes-1<<endl;
			//			cout<< "  "<<recv<<endl;

			if(recv == 255){
				numBytes = 0;
				//						cout<<"newline"<<endl;
			}

			if(numBytes == 4){
				//			numBytes = 0;
				//		cout<<(int)bytesReturned2[0]<<"  "<<(int)bytesReturned2[1]<<"  "<<(int)bytesReturned2[2]<<"  "<<(int)bytesReturned2[3]<<"  "<<endl;
				if(!connectedToArduino){
					slog("Arduino connected");
					connectedToArduino = true;
				}
				msgTimer = ofGetElapsedTimeMillis();
				//				cout<<(bool)bytesReturned2[3]<<endl;
				if(button != (bool)bytesReturned2[3]){
					if(!button){
						sound.play();

						cout<<"Play"<<endl;
					} else {
						sound.stop();
						curTime = 0;
						cout<<"Pause"<<endl;
					}
					button = (bool)bytesReturned2[3];
				}
				for(int i=0;i<3;i++){
					curAcc0[i] = bytesReturned2[i];
					acc0.push_back(curAcc0[i]);

				}


				/*
				 int msg = atoi((const char *)bytesReturned2);
				 //	cout<<msg<<"   "<<endl;
				 //	cout<<bytesReturned2[0]<<endl;
				 //						cout<<"recv "<<msg<<"    "<<bytesReturned2[0]<<"  "<<bytesReturned2[1]<<"  "<<bytesReturned2[2]<<"  "<<bytesReturned2[3]<<endl;
				 lastRecv = msg;
				 if(msg == 9999){
				 msgLock = false;
				 //	framesSinceLastResponse = 0;
				 //	responseTime = millis();
				 msgTimer = ofGetElapsedTimeMillis();
				 arduinoBufferSize = 0;
				 shortSum = -1;
				 }
				 else if(msg == 9998){
				 //acc response
				 // int n[][];
				 recvAcc[0] = 0;
				 recvAcc[1] = -1;
				 recvAcc[2] = -1;
				 //  append(acc,n);
				 }
				 else if( recvAcc[0] == 0 && recvAcc[1] == -1){
				 recvAcc[0] =(int) bytesReturned2[1];
				 recvAcc[1] =(int) bytesReturned2[2];
				 recvAcc[2] =(int) bytesReturned2[3];

				 if(recvAcc[0] > 100 && recvAcc[0] < 700 && recvAcc[1] > 100 && recvAcc[1] < 700 && recvAcc[2] > 100 && recvAcc[2] < 700){
				 for(int i=0;i<3;i++){
				 acc0.push_back(recvAcc[i]-350);
				 curAcc0[i] = recvAcc[i]-350;

				 }
				 }
				 if(acc0.size() > 200){
				 acc0.erase(acc0.begin(),acc0.begin()+ 2);
				 }
				 recvAcc[0] = -1;
				 recvAcc[1] = -1;
				 recvAcc[2] = -1;
				 */
			}
		}


		if(gotMsg){
			//	cout<<duskValues[0]<<endl;

			unsigned char send[65];
			for(int i=0;i<64;i++){
				send[i] = duskValues[i];
			}
			send[64] = 255;
			serial.writeBytes(send, 65);
		}

		if(ofGetElapsedTimeMillis() - msgTimer > MSG_TIME_OUT){
			//cout<<"Reconnected, last msg: "<<lastSend<<", last recv: "<<lastRecv<<"   millis: "<<ofGetElapsedTimeMillis()<<", msgTimer: "<<msgTimer<<" sum "<<(ofGetElapsedTimeMillis() - msgTimer)<<" > "<<MSG_TIME_OUT<<endl;
			//		connectedToArduino = false;
			receivingAcc =false;
			msgQueue.clear();
			//		if(msgQueue.size() < 2)
			setOutputShort(duskValues);
			serial.writeByte(0);
			msgTimer = ofGetElapsedTimeMillis();
		}
	};

	//serial.writeByte(1);

	/*
	 if(msgQueue.size() < 2 ){

	 //  queueMessage(GET_ACC);
	 int updateDuskChannels = 0;
	 for(int i=0;i<64;i++){

	 }
	 for(int i=0;i<64;i++){
	 if(duskSendValues[i] != duskValues[i]){
	 updateDuskChannels ++;
	 }
	 }
	 if(updateDuskChannels > 20){
	 if(shortSum == -1){
	 setOutputShort(duskValues);
	 updateDusk();
	 queueMessage(ASK_RESPONSE);
	 }
	 }
	 else if(updateDuskChannels>0) {
	 for(int i=0;i<64;i++){
	 setOutput(i,duskValues[i]);
	 }
	 updateDusk();

	 //		queueMessage(DUSK_UPDATE);
	 }

	 if(updateDuskChannels>0){
	 //	updateDusk();
	 for(int i=0;i<64;i++){
	 duskSendValues[i] = duskValues[i];
	 }
	 }
	 }

	 parseQueue();*/








}

void testApp::slog(string s, bool end){
	if(end){
		cout<<s<<endl;
		logString += s+"\n";
	}
	else {
		cout<<s;
		logString += s;
	}
}

void testApp::loadSound(string file){
	slog("Load sound: \"	"+file+"\"...", true);
	sound.loadSound(file);
	soundLoaded = sound.bLoadedOk;
	if(soundLoaded)
		slog("Sound loaded. Length: "+ofToString((sound.length/sound.internalFreq),0)+" sec, freq: "+ofToString(sound.internalFreq,0));
	else
		slog("Sound could not be loaded!");
	sound.stop();

}

void testApp::setOutput(int output, int val){
	if(val > 254 ){
		val = 254;
	}
	if(val < 0){
		val = 0;
	}
	if(duskOutputs[output] == val){

	}
	else {
		queueMessage(SET_CHANNEL_BYTE);
		queueMessage(output);
		queueMessage((float)val*masterVibration);
		duskOutputs[output] = val;
	}
}

void testApp::setOutputShort(int val[]){
	queueMessage(SET_CHANNEL_SHORT_BYTE);
	shortSum = 0;
	for(int i=0;i<64;i++){
		if(val[i] > 254)
			val[i] = 254;
		queueMessage((float)val[i]*masterVibration);
		shortSum += val[i];
	}
}


void testApp::setOutputGroup(int outputIn, int outputOut, int val){
	if(val > 254 ){
		val = 254;
	}
	if(val < 0){
		val = 0;
	}
	bool doUpdate = false;
	for(int i=outputIn; i<= outputOut; i++){
		if(duskOutputs[i] != val){
			doUpdate = true;
			duskOutputs[i] = val;
		}
	}
	if(doUpdate) {

		queueMessage(SET_CHANNEL_GROUP_BYTE);
		queueMessage(outputIn);
		queueMessage(outputOut);
		queueMessage((float)val*masterVibration);
	}
}


void testApp::updateDusk(){
	queueMessage(DUSK_UPDATE);
}


void testApp::writeSerial(int msg){
	if(serialConnected){
		//		if(serial.available() > 0){
		//			serial.flush(true, false);
		//		}
		//				cout<<"write                                "<<msg<<endl;
		//		while(!serial.writeByte(msg));
		//				serial.writeByte(msg);
		lastSend = msg;
		lastRecv *= -1;
		arduinoBufferSize ++;
	}
}


void testApp::queueMessage(int msg){
	msgQueue.push_back(msg);
}

void testApp::parseQueue(){
	int start = ofGetElapsedTimeMillis();
	while(msgQueue.size() > 0 && !msgLock && ofGetElapsedTimeMillis() - start < 7)
	{
		lstMsg = msgQueue[0]; //queue the first message on the stack
		if(lstMsg > 10) lstMsgType = lstMsg; //message greater than 10 specify message types

		writeSerial(lstMsg);

		msgTimer = ofGetElapsedTimeMillis();
		msgQueue.erase(msgQueue.begin());

		if(lstMsg == ASK_RESPONSE){
			//			writeSerial(GET_ACC);
			//			writeSerial(ASK_RESPONSE);
			//					writeSerial(GET_ACC);
			msgLock = true;
		}
		else {
			if(arduinoBufferSize > 120){
				//Ask for response at arduino
				cout<<" buffer"<<endl;
				writeSerial(ASK_RESPONSE);
				msgLock = true;

			}
		}
		//responseTime = ofGetElapsedTimeMillis();

	}
	//  println(millis() - start);
	if(ofGetElapsedTimeMillis() - msgTimer > MSG_TIME_OUT && msgLock){
		if(serialConnected && connectedToArduino)
			//	cout<<"Reconnected, last msg: "<<lastSend<<", last recv: "<<lastRecv<<"   millis: "<<ofGetElapsedTimeMillis()<<", msgTimer: "<<msgTimer<<" sum "<<(ofGetElapsedTimeMillis() - msgTimer)<<" > "<<MSG_TIME_OUT<<endl;
			//		connectedToArduino = false;
			receivingAcc =false;
		msgQueue.clear();
		//		if(msgQueue.size() < 2)
		setOutputShort(duskValues);
		writeSerial(ASK_RESPONSE);
		msgLock = true;
		msgTimer = ofGetElapsedTimeMillis();
	}
	if(!msgLock && msgQueue.size() == 0){
		//Ask for response at arduino
		writeSerial(ASK_RESPONSE);
		msgTimer = ofGetElapsedTimeMillis();
		msgLock = true;
		//	responseTime = millis();
	}
}

